/*
 * lib.h
 *
 *  Created on: 2010.06.27
 *      Author: Tadas
 */

#ifndef LIB_H_
#define LIB_H_

#include <cmath>

#define EPS 1e-8
#define JMAX 16

using namespace std;

const int lygSkaic = 2;

double **allocate2d(int eil, int stl)
{
	double **mas;

	mas = new double *[eil];
	assert(mas!=NULL);

	for (int i=0; i<eil; i++)
		{
			mas[i] = new double [stl];
			assert(mas[i]!=NULL);
		}

	return mas;
}

void step(double *y, double t, double h, void (*func)(double, double *, double *)){
	int i;
	double th, tt, t6;
	
	double dym[lygSkaic], dyt[lygSkaic], yt[lygSkaic], f[lygSkaic];
	
	tt = 0.5 * h;
	t6 = h/6.0;
	
	th = t + tt;
	func(t, y, f);
	
	for (i = 0; i < lygSkaic; i++){
		y[i] = y[i] + tt*f[i];
	}
	
	func(th, yt, dyt);
	
	for(i = 0; i < lygSkaic; i++){
		y[i] = y[i] + tt * dyt[i];
	}
	
	func(th, yt, dym);
	
	for(i = 0; i < lygSkaic; i++){
		yt[i] = y[i] + h * dym[i];
		dym[i] += dyt[i];
	}
	
	func(t + h, yt, dyt);
	
	for(i = 0; i < lygSkaic; i++){
		y[i] += t6 * (f[i] + 2 * dym[i] + dyt[i]);
	}
	
}

void RK4solver(double **y, double *x, int ipoint, void (*func)(double, double *, double *)){
	double yNew[lygSkaic], yOld[lygSkaic], yt[lygSkaic];
	
	double hh, h, temp;
	
	int j, i, k, flag, in;
	
	for(i = 1; i < ipoint; i++){
		h = x[i] - x[i - 1];
		
		for(j = 0; j < JMAX; j++){
			for(k = 0; k < lygSkaic; k++){
				yt[k] = y[i - 1][k];
			}
			
			if(j == 0){
				step(yt, x[i - 1], h, func);
				for(k = 0; k < lygSkaic; k++){
					yOld[k] = yt[k];
				}
			}
			else {
				for(k = 0, in = 1; k < j; k++){
					in <<= 1;
				}
				hh = h/double(in);
				
				for(k = 0; k < in; k++){
					step(yt, x[i - 1] + k*hh, hh, func);
				}
				
				for( k = 0; k < lygSkaic; k++){
					yNew[k] = yt[k];
				}
				
				flag = 0;
				
				for(k = 0; k < lygSkaic; k++){
					if(fabs(yOld[k] - yNew[k]) > EPS){
						flag  = 1;
						break;
					}
				}
				
				if( flag == 0){
					for(k = 0; k < lygSkaic; k++){
						y[i][k] = yNew[k];
					}
					break;
				}
				
				for(k = 0; k < lygSkaic; k++){
					yOld[k] = yNew[k];
				}
			}
		}
		
		if( j == JMAX){
			cout << "Per daug zingnsiu funkcijoje RK4Solver" << endl;
			for(k = 0; k < lygSkaic; k++){
				y[i][k] = yNew[k];
			}
		}
	}
}



#endif /* LIB_H_ */
